/* Author: Marco Tezzele */

/* A* algorithm that solves the single-source shortest path problem for
 * a graph with nonnegative edge path costs. */

/* For references see http://en.wikipedia.org/wiki/A*_search_algorithm */

#include <stdio.h>

#define N_NODES        4 /* Number of nodes */
#define INT_MAX      100 /* Intero massimo per inizializzare distanceVector */
#define START_NODE     0 
#define GOAL_NODE      3 

float heuristic_cost_estimate(int, int, int, float[][N_NODES], float[][N_NODES]);
void reconstruct_path(int[], int, float);
int path_length(int, int[]);

int aStar() {

	int i, j, m, x, y;
	float weightsMatrix[N_NODES][N_NODES]; // matrice dei pesi variabili
	float timesMatrix[N_NODES][N_NODES];   // matrice dei tempi
	int came_from[N_NODES];      // vettore contenente i nodi da cui provengo
	int visitedVector[N_NODES]; /* vettore identifica il nodo:
	 * 0 = openset
	 * 1 = closedset
	 * -1 = altrimenti */
	float g_score[N_NODES];  // costo dallo start
	float h_score[N_NODES];  // costo stimato fino al goal
	float f_score[N_NODES];  // costo totale
	int tentative_is_better; // (in C non esiste bool)
	float tentative_g_score;
	int length;

	/* Controllo se il file viene aperto correttamente. */
	FILE * fileWeights = fopen("weights.txt", "r");
	FILE * fileTimes = fopen("times.txt", "r");
	if (fileWeights == NULL || fileTimes == NULL) {
		printf("C'e' stato un errore nell'apertura del file\n");
		return 1;
	}

	/* Legge i valori da file */
	for (i = 0; i < N_NODES; ++i) {
		for (j = 0; j < N_NODES; ++j) {
			fscanf(fileWeights, "%f", &weightsMatrix[i][j]);
			fscanf(fileTimes, "%f", &timesMatrix[i][j]);
		}
	}
	fclose(fileWeights);
	fclose(fileTimes);

	/* Inseriamo tutti i nodi al di fuori di openset e closedset e
	 * inizializziamo came_from con -1. */
	for (i = 0; i < N_NODES; ++i) {
		visitedVector[i] = -1;
		came_from[i] = -1;
	}

	/* Inserisco il nodo di partenza nell'openset, fisso il costo dallo start = 0
	 * e calcolo l'euristica corrispondente. */
	length = 0; // essendo il primo nodo
	visitedVector[START_NODE] = 0;
	g_score[START_NODE] = 0;
	h_score[START_NODE] = heuristic_cost_estimate(START_NODE, GOAL_NODE, length,
	        weightsMatrix, timesMatrix);
	f_score[START_NODE] = h_score[START_NODE];
	
	for (;;) {

		/* Cerco il nodo con costo totale minore tra i nodi nell'openset. */
		for (m = INT_MAX, i = 0; i < N_NODES; ++i) {
			if (visitedVector[i] == 0 && f_score[i] <= m) {
				m = f_score[j = i];
			}

		}

		/* Se sono arrivato ricostruisco il cammino e termino */
		if (j == GOAL_NODE) {
			reconstruct_path(came_from, came_from[GOAL_NODE],
			        g_score[GOAL_NODE]);
			break;
		}
		/* inserisco il nodo nel closedset */
		visitedVector[j] = 1;

		/* Scorro tutti i nodi raggiungibili da j */
		for (i = 0; i < N_NODES; ++i) {
			if (weightsMatrix[j][i]) {
				if (visitedVector[i] == 1) {
					continue;
				}

				/* Calcolo la lunghezza fino a j e il nuovo tentativo per g_score
				 * e' uguale al costo per arrivare in j + il tempo per arrivare in i
				 * + il peso per arrivare a i diviso per il numero di occupanti, che
				 * corrisponde alla lunghezza del cammino fino a j + 1. */
				length = path_length(j, came_from);
				tentative_g_score = g_score[j] + timesMatrix[j][i]
				        + weightsMatrix[j][i];

				// weightsMatrix[j][i]/(float)(length + 1); per i pesi variabili, da ripensare

				if (visitedVector[i] == -1) {
					/* Inserisco i nell'openset */
					visitedVector[i] = 0;
					tentative_is_better = 1;
				} else if (tentative_g_score < g_score[i]) {
					tentative_is_better = 1;
				} else {
					tentative_is_better = 0;
				}

				/* ho trovato il giusto i, aggiorno tutti i vettori */
				if (tentative_is_better) {
					came_from[i] = j;
					g_score[i] = tentative_g_score;
					h_score[i] = heuristic_cost_estimate(i, GOAL_NODE,
					        length + 1, weightsMatrix, timesMatrix); // + 1 perche' arrivo in i
					f_score[i] = g_score[i] + h_score[i];
				}
			}
		}
	}
	
	return 0;
}

/* Funzione euristica che stima il costo dal nodo corrente al nodo obiettivo.
 * Deve essere non crescente e sottostimare il reale costo per trovare soluzioni ottime.
 * Utilizziamo un dijkstra's algorithm dividendo la distanza per la lunghezza totale del 
 * cammino (lunghezza fino al current_node + lunghezza dal current_node al goal_node)
 * input:
 * 1: nodo corrente
 * 2: nodo obiettivo
 * 3: lunghezza del cammino fino a current_node
 * 4: matrice dei pesi
 * 5: matrice dei tempi
 * output: 
 * stima del costo per giungere all'obiettivo
 */
float heuristic_cost_estimate(int current_node, int goal_node, int length1,
        float weightsMatrix[][N_NODES], float timesMatrix[][N_NODES]) {

	int i, j, m;
	float distanceVector[N_NODES];   // vettore delle distanze
	int minPathsVector[N_NODES];     // vettore contenente i percorsi minimi
	int visitedVector[N_NODES]; // vettore booleano che serve a marcare i nodi visitati
	int length2;
	float cost_estimate;

	/* Inizializzo il vettore delle distanze con una distanza massima non
	 * raggiungibile e il vettore dei nodi visitati. */
	for (i = 0; i < N_NODES; ++i) {
		distanceVector[i] = INT_MAX;
		visitedVector[i] = 0;
		minPathsVector[i] = -1;
	}

	/* Fisso la prima distanza uguale a zero visto che parto dal vertice in
	 * questione e fisso minPathsVector del primo nodo uguale a -1 per capire
	 * che parto da questo nodo. */
	distanceVector[current_node] = 0;
	minPathsVector[current_node] = -1;

	for (;;) {

		/* Cerca il nodo con distanza minima tra quelli non ancora visitati. */
		for (m = INT_MAX, i = 0; i < N_NODES; ++i) {
			if (!visitedVector[i] && distanceVector[i] <= m) {
				m = distanceVector[j = i];
			}
		}
		/* Nodo visitato */
		visitedVector[j] = 1;

		/* Quando raggiungo il vertice d'arrivo o arrivo al nodo piu' lontano
		 * raggiungibile (ARRIVAL_NODE in questo caso irraggiungibile) mi fermo */
		if (j == goal_node || m == INT_MAX
		)
			break;

		/* Aggiorno la distanza per raggiungere l'i-esimo nodo in base al
		 * seguente principio: sono al nodo j, per tutti i nodi i raggiungibili
		 * da j se il costo per raggiungere direttamente i (la distanza da current_node)
		 * e' maggiore del costo del percorso che passa per j allora forzo il passaggio
		 * per j con minPathsVector[i]=j e assegno la nuova distanza. */
		for (i = 0; i < N_NODES; ++i) {
			if (weightsMatrix[j][i]
			        && distanceVector[i]
			                > distanceVector[j] + timesMatrix[j][i]
			                        + weightsMatrix[j][i]) {
				distanceVector[i] = distanceVector[j] + timesMatrix[j][i]
				        + weightsMatrix[j][i];
				minPathsVector[i] = j;
			}
		}

	}

	length2 = path_length(goal_node, minPathsVector);

	cost_estimate = distanceVector[goal_node] / (float) (length1 + length2);

	return cost_estimate;
}

/* Funzione che ricostruisce il path e lo stampa. 
 * input: 
 * 1: vettore came_from con i nodi di provenienza
 * 2: nodo corrente
 * 3: costo per giungere al nodo obiettivo
 */
void reconstruct_path(int came_from[], int current_node, float path_cost) {

	int i = 0;
	int j;
	int inverse_path[N_NODES];

	/* Ricostruisco il path facendo il percorso in came_from al contrario */
	inverse_path[i] = current_node;
	while (inverse_path[i] > -1) {
		inverse_path[i + 1] = came_from[inverse_path[i]];
		++i;
	}

	printf("Il percorso minimo per andare da %d a %d e' il seguente:\n",
	        START_NODE, GOAL_NODE);
	/* Questo ciclio serve per leggere il vettore dalla fine all'inizio evitando
	 * il primo termine che e' -1. */
	for (j = i - 1; j >= 0; --j) {
		printf("%d -> ", inverse_path[j]);
	}
	printf("%d\n", GOAL_NODE);
	printf("ha un costo pari a: %f\n", path_cost);
	printf("e una lunghezza pari a: %d\n", i);

	return;
}

/* Funzione che calcola la lunghezza del cammino dallo start (came_from[START_NODE] = -1) 
 * fino a current_node.
 * input:
 * 1: nodo corrente
 * 2: vettore contenente i nodi da cui provengo
 * output: 
 * lunghezza cammino
 */
int path_length(int current_node, int came_from[]) {

	int length = 0;
	int p; // serve per scorrere tutti i nodi fino allo start

	p = came_from[current_node];
	while (p > -1) {
		p = came_from[p];
		++length;
	}

	return length;
}
